/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalModule;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO;
import edu.wpi.first.wpilibj.DriverStationEnhancedIO.EnhancedIOException;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.can.CANTimeoutException;
import java.util.Timer;
import java.util.TimerTask;
//import org.thecatattack.System451.Communication.Dashboard.*;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.Joystick;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the SimpleRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Team997ArmTests extends SimpleRobot {

    private TrackerDashboard tdb = new TrackerDashboard(new Solenoid(7, 1));
    private CANJaguar m_basicArm;
    private DriverStationEnhancedIO m_enhancedIO;
    private double m_shoulderValue = 0;
    private static final double kMinShoulderVoltage = 0.05;
    private static final double kMaxShoulderVoltage = 3.3;
    private static final double kMinShoulderPosition = 0.1;
    private static final double kMaxShoulderPosition = 0.7;
//    private ZomBDashboard m_zomB = ZomBDashboard.getInstance(ZomBModes.DBPacket, "10.9.97.100");
    private static final boolean kEnableSmartDashboard = true;
    private static final int kDigitalSlot = 4;
    private I2C m_UltraSonic = new I2C(DigitalModule.getInstance(kDigitalSlot), 0xE0);
    private Joystick m_leftJoy = new Joystick(1);

    public void robotInit() {
        System.out.println("Build complete!");

        DriverStationPrinter.getInstance().println("Build complete!");
        if (kEnableSmartDashboard) {
            SmartDashboard.init();
        }
        try {
            m_basicArm = new CANJaguar(2, CANJaguar.ControlMode.kPosition);
            m_basicArm.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
            m_basicArm.setPID(300, 0, 20);
            m_basicArm.configPotentiometerTurns(1);
            m_basicArm.configSoftPositionLimits(kMinShoulderPosition, kMaxShoulderPosition);

            m_basicArm.enableControl();

        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println("CAN Init Error!");
            //DriverStationPrinter.getInstance().println(e.getMessage());
            e.printStackTrace();
        }

        m_enhancedIO = DriverStation.getInstance().getEnhancedIO();

        Timer mainTimer = new Timer();
        mainTimer.schedule(new TimerTask() {

            public void run() {
                updateSmartDashboard();
            }
        }, 1000, 100);
    }

    /**
     * This function is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
    }

    /**
     * This function is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        while (isEnabled() && isOperatorControl()) {
            try {

                m_shoulderValue = m_enhancedIO.getAnalogIn(2);
            } catch (EnhancedIOException e) {
                DriverStationPrinter.getInstance().println("Analog Error!");
                DriverStationPrinter.getInstance().println(e.getMessage());
                e.printStackTrace();
            }

            m_shoulderValue -= kMinShoulderVoltage;
            m_shoulderValue /= (kMaxShoulderVoltage - kMinShoulderVoltage);
            if (m_shoulderValue < 0) {
                m_shoulderValue = 0;
            }
            if (m_shoulderValue > 1) {
                m_shoulderValue = 1;
            }

            double dMotorValueShoulder = m_shoulderValue;
            dMotorValueShoulder *= (kMaxShoulderPosition - kMinShoulderPosition);
            dMotorValueShoulder += kMinShoulderPosition;

            try {
                m_basicArm.setX(dMotorValueShoulder);

            } catch (CANTimeoutException e) {
                DriverStationPrinter.getInstance().println(e.getMessage());
                e.printStackTrace();
            }

            byte[] tempArray = new byte[1];
            if (m_leftJoy.getTrigger()) {
                if (m_UltraSonic.read(0x00, 1, tempArray)) {
                    DriverStationPrinter.getInstance().println("Error Reading 01");
                }
                else {
                    if (tempArray[0] == 10) {
                        if (m_UltraSonic.write(0x00, 0x52)) {
                            DriverStationPrinter.getInstance().println("Error Writing 02");
                        }
                        else {
                            boolean isInLoop = true;
                            while (isInLoop) {
                                try {
                                    Thread.sleep(5);
                                }
                                catch (InterruptedException e) {}
                                if (!m_UltraSonic.read(0x00, 1, tempArray)) {
                                    if (tempArray[0] == 10) {
                                        isInLoop = false;
                                    }
                                }
                            }
                            
                            for (int i = 1; i <= 17; i++) {
                                byte[] tempArray2 = new byte[1];
                                m_UltraSonic.read(0x01 + (i * 2), 1, tempArray);
                                m_UltraSonic.read(0x02 + (i * 2), 1, tempArray);
                                double speedOfSound = 1126 * 12 / 1000000.0;
                                double distance = (((((int) tempArray[0]) << 8) | ((int) tempArray2[0])) / speedOfSound) / 2;
                                SmartDashboard.log(distance, "Ultra" + i);
                            }
                            m_UltraSonic.read(0x01, 1, tempArray);
                            SmartDashboard.log(Integer.toHexString(tempArray[0]), "Light");
                        }
                    }
                }
                while (m_leftJoy.getTrigger());
            }

            getWatchdog().feed();
        }
    }

//    public void updateZomB() {
//        if (m_zomB.CanSend()) {
//            try {
//                DriverStationPrinter.getInstance().println("Writing to ZomB!");
//                m_zomB.Add("MotorAmps", m_basicArm.getOutputCurrent());
//                m_zomB.Add("text", "Running");
//                m_zomB.Send();
//            } catch (CANTimeoutException ex) {
//                DriverStationPrinter.getInstance().println(ex.getMessage());
//                ex.printStackTrace();
//            }
//        }
//        else{
//            DriverStationPrinter.getInstance().println("Can't write to ZomB!");
//        }
//    }
    public void updateSmartDashboard() {
        try {
            SmartDashboard.log(m_basicArm.getOutputCurrent(), "MotorAmps");
            SmartDashboard.log(true, "Test");
        } catch (CANTimeoutException ex) {
            ex.printStackTrace();
        }
    }

    private void logSmartDashboard(String key, String value) {
        if (!kEnableSmartDashboard) {
            return;
        }
        int nRetCode = SmartDashboard.log(value, key);

        if (nRetCode != SmartDashboard.SUCCESS) {
            System.out.println("Error: " + SmartDashboard.diagnoseErrorCode(nRetCode));
        }
    }
}
